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"Pseudo  stAte  measurements"  are  constructed  to  make  the 
measurement  (geometry)  model  linear  in  the  state.  In  the  past,  linear 
measurements  have  often  proved  to  give  better  state  estimates  than  non¬ 
linear  measurements.  They  arc  nonlinear  functions  of  the  actual  measure¬ 
ment  model  bias  parameters  and  are  constructed  to  be  linear  functions  of 
the  state  variables  or  to  vanish  in  the  absence  of  model  or  measurement 
error.  Some  examples  of  constructing  pseudo  state  measurements  are 
given  in  the  paper.  Recursive  filter  equations  are  derived  using  the  pseudo 
state  measurements  and  including  colored  (Markov)  measurement  noise 
and  unestimated  state  and  measurement  model  parameters.  The  filter  esti¬ 
mates  minimize  the  usual  weighted  least  squares  cost  function  with  corre¬ 
lated  state  and  pseudo  state  measurements.  The  filter  is  linear  by  con¬ 
struction.  Higher  order  partial  derivatives,  if  retained,  would  appear  only 
in  the  computation  of  error  variance  and  covariance  matrices. 
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SECTION  I 


INTRODUCTION 


Nonlinear  problem  geometries  often  assume  that  the  measurements 

are  related  to  the  state  using  the  nonlinear  relation  Z  =  h  (X  )  +  N  .  With 

°  n  n'  n  n 

this  nonlinear  form  of  the  measurement  model  employed,  the  current  optimal 
estimate  can  only  approximately  be  represented  as  a  linear  combination  of 
the  measurements.  The  usual  procedure  is  either  to  iterate  the  solution 
until  acceptable  accuracy  is  obtained  or  simply  to  ignore  the  partial  deriva¬ 
tives  of  hn(Xn)  that  are  higher  than  the  first  (Ref.  1).  In  this  latter  case, 
a  direct  computation  of  the  errors  caused  by  the  omission  of  the  second 
derivatives  in  some  applications  exceeds  the  measurement  noise  by  an 
order  of  magnitude.  Hence  a  serious  convergence  problem  may  arise  with 
this  formulation.  Techniques  for  using  the  measurement  me,  1el  second 
derivatives  are  given  in  Ref.  2. 

A  new  approach  to  nonlinear  filtering  with  correlated  measurement 
noise  is  presented  in  this  paper.  This  approach,  using  pseudo  state  mea¬ 
surements,  differs  from  the  usual  approach  of  the  "extended  Kalman"  filter. 
The  latter  approach  requires  the  computation  of  nonlinear  residuals  and 
uses  nonlinear  propagation  of  the  state.  The  approach  defined  in  this  paper 
uses  a  nonlinear  transformation  of  the  actual  measurements  and  the  a  priori 
state  variables  to  obtain  pseudo  state  measurements.  The  analysis  in  this 
paper  is  general  in  that  all  such  functions  are  permitted,  including  functions 
that  vanish  in  the  noise- free  case.  The  principal  advantage  results  when  the 
pseudo  state  measurements  are  constructed  to  be  linear  functions  of  the 
state  variables. 

All  nonlinearity  in  the  measurement  model  is  restricted  to  the 
computation  of  the  covariance  and  variance  error  matrices.  These  non- 
linearities  are  unavoidable  and  do  not  appear  to  be  a  source  of  concern. 
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The  usual  weighted  least  squares  cost  function  of  correlated 
residuals  is  adopted  as  the  cost  function  to  be  minimized.  The  use  of  pseudo 
state  measurements  has  been  tested  to  a  limited  extent  by  the  author,  using 
both  simulated  and  actual  data  with  good  results. 

The  derivation  using  pseudo  state  measurements  in  this  paper 
includes  the  derivation  in  Ref.  3  as  a  special  case.  The  equations  (see 
Appendix  A)  degenerate  to  the  usual  Kalman  filter  by  setting  S,  o,  S,  V, 

D,  G,  L,  Ctf,  and  B  to  zero  and  to  I. 
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SECTION  II 


EXAMPLES  OF  PSEUDO  STATE  MEASUREMENTS 

Pseudo  state  measurements  £n  are  constructed  as  nonlinear  functions 

of  the  actual'  measurements  Z  and  the  a  priori  state  estimate  X  as  t  - 
_  n  r  n  m 

t,n(Z  ,  X^).  It  is  convenient  to  construct  pseudo  state  measurements  to  be 

linear  functions  of  the  state  variables  The  pseudo  state  residuals 

are  then  Ar  =  £  (Z  ,  X  )  -  i  (X  ),  where  i  (X  )  =  $£  X  are  linear  func- 
n  n  n  n'  nv  n'  n  n  n  n 

tions  of  the  state  variables  by  construction. 

Several  examples  will  be  given  to  illustrate  the  construction  of 

pseudo  state  measurements.  Consider  that  thj  state  variables  include  X^, 

•  •  • 

X^>  Xy  Xj,  X 2»  and  X^j  •  •  .  »  •  It  is  possible  th?t  three  simultaneous 
position  measurements  can  be  made  such  that  X^,  X^,  and  X^  can  be  cal¬ 
culated  from  the  three  measurements.  Examples  of  this  include  trilatera- 
tion,  where  three  simultaneous  range  measurements  are  made  from  three 
different  stations.  (In  this  context  a  station  may  denote  either  a  ground  sta¬ 
tion,  a  ship,"  or  a  satellite  whose  coordinates  are  known  approximately.  ) 

Other  examples  can  be  constructed  wherein  exactly  three  position  measure¬ 
ments  involving  range,  angle,  and/or  direction  cosines,  etc.  ,  are  mea¬ 
sured  from  which  X^,  X 2,  and  X^  can  be  calculated  in  the  ideal  (error-free) 
case  (Refs.  4  and  5). 

A  simple  example  occurs  when  range,  azimuth,  and  elevation 
angles  R,  A,  E  are  measured  from  a  station.  Then 

=  R  cos  A  cos  E  (.?  i  =  Xj) 

-  R  sin  A  cos  E  (f  2  =  X.,) 

£3  =  R  sin  E  (i  3  =  X3) 

arc  pseudo  state  measurements  since  these  are  nonlinear  functions  of  the 
actual  measurements.  This  is  a  special  case  since  a  priori  state  variables 


were  not  required  in  the  construction.  In  general*  it  is  not  possible  to  find 
a  nonlinear  transformation  of  the  measurements  that  will  be  linear  functions 
of  the  state  variables.  Consider  the  case  where  two  angle  measurements 
are  made  from  a  single  station.  Then  the  pseudo  state  measurements  (PSMs) 
become 

=  R  cos  A  cos  E  (ij  =  X 

=  R  sin  A  cos  E  =  X ^ ) 

=  R  sin  E  (i3  =  X3) 

_  _2  —2  —2  1  /2 

where  R  =  (X^  +  X^  +  X^)  is  constructed  using  the  a  priori  state  vector. 

If  E,  A  are  measured  at  several  stations,  then  the  same  procedure  may  be 
followed  for  each  station.  This  procedure  extends  the  use  of  the  a  priori 
data  to  the  measurement  model.  It  may  be  preferable  in  this  example  to 
omit  t,2  or  £3  since  there  are  only  *wo  measurements.  The  use  of  only  two 
PSMs  may  avoid  some  matrix  inversion  problems  in  the  gain  computation. 

An  alternate  construction  of  the  PSM  for  angle  only  tracking  data  is  as 
follows: 

=  -Xj  sin  A  +  X2  cos  A  (ij  =  0) 

4 2  =  Xj  sin  E  cos  A 

+  X2  sin  E  sin  A  -  X^  cos  E  (f  2  =  0) 

This  technique  has  the  further  advantage  that'  9  4/3X.  are  independent  of  X^. 

In  the  case  where  R  is  measured  at  one  or  more  stations,  it  is  only 
necessary  to  construct  E,  A  from  the  a  priori  data  in  order  to  obtain  ^ 
and  £3  (°r  any  one  of  the  three).  A  similar  procedure  can  be  constructed 
for  velocity,  acceleration,  and  attitude  measurements  obtained  in  the  sensor 
coordinate  system.  These  measurements  may  be  linearly  related  to  the 
state  vector. 
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For  example,  when  the  measurements  are  R,  E,  and  A,  then  the 
pseudo  state  velocity  measurements  may  be  constructed  as 

-  (k.  E,  A;  R,  E,  A)  (£.  =  X.)  (i  =1,2,3) 

where  R,  E,  and  A  are  calculated  from  X,  Y,  and  Z.  In  a  similar  fashion, 
vehicle  measurements  of  acceleration  and  attitude  rate  may  be  linearly 
related  to  the  state  vector. 


SECTION  III 


FILTER  COMPARISON 


The  pseudo  state  measurement  filter  (PSM)  can  be  compared  with 
the  extended  Kalman  filter  (EKF)  in  the  following  special  case  involving 
white  measurement  noise  and  unestimated  parameters.  The  result  is 
given  below: 

EKF 


I<  =  M  rT(R  +  <£  M  fjF) 
n  n  n'  n  n  n  n ' 


P  =  (I  -  K  f£  )  M 
n  n  n  n 


X  =  X  +  K  [t,  -  t  (X  )] 
n  n  n  l*n  nv  nn 


PSM 

K  =  M  {.V  -.ye  )T  [<Q  R  &T  +  (Se  -JiT)1]"1 

n  n  n  n  1  n  n  n  '  n  n'  n'  n  n 

P  *  fl  -  K  (5f  -.yi’  )]  M 
n  n  n  nn  1  n 


X  +  K  ( l  -  <£  X  ) 
n  n  n  n  n7 


where  M  <t>  P  ,  4>  +  Q  and  X  are  evaluated  using  the  nonlinear  formu- 

iation.  The  balance  of  the  symbols  arc  defined  in  Appendix  A.  Note  that  no 

errors  are  introduced  in  the  PSM  formulation  as  a  result  of  neglecting 

second-order  partial  derivatives  of  In  one  of  the  examples  of 

PSMs,  it  follows  that  both  //"  and  JC  are  constants.  The  EKF  formula- 

n  n 

t;<>n  sutlers  from  the  fact  that  !/*  are  not  constants. 
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APPENDIX  A 


SUMMARY 

The  filtering  concept  may  be  explained  with  reference  to  the  block 
diagram  (Fig.  A-l),  which  shows  seven  computational  blocks  denoted  as: 

Block  1.  State  propagation 

Block  2.  Pseudo  measurements 

Block  3.  Set  up  computations 

Block  4.  Cost  function 

Block  5.  Gain  and  covariance  computation 

Block  6.  Step  initializa.tion 

Block  7.  Filter  performance  evaluation 

The  previous  best  estimate  of  the  state  vector  is  propagated  in 

Block  1  to  the  time  t  when  the  measurements  are  made.  Pseudo  state  mea- 

n  r- 

surements  and  pseudo  state  residuals  are  calculated  in  Block  2.  The  gain 

matrix  Kn  is  computed  recursively  in  Block  5.  The  gain  matrix  scales  the 

pseudo  state  measurement  residuals  to  obtain  the  improvement  increment 

to  be  added  to  X  .  The  error  covariance  matrix  of  the  estimate  is  also 
n 

computed  in  Block  5. 

The  computation  of  the  gain  matrix  requires  that  matrices  M,  R, 

2,  and  C  be  computed.  This  is  performed  in  set  up  (Block  3). 

Block  6,  step  initialization,  is  included  so  that  the  required  input 
matrices  to  Block  5  (G,  Y,  and  L)  may  be  computed  recursively. 

Blocks  4  and  7  are  included  so  that  the  filter  performance,  includ¬ 
ing  dynamics  and  measurement  modeling,  may  be  evaluated.  Note  that 
inclusion  of  these  blocks  is  not  required  for  the  recursive  filter  operation. 
These  may  be  included  when  the  computational  capability  permits  and  when 
there  is  some  question  as  to  the  adequacy  of  the  modeling  process. 


Figure  A-i.  System  Block  Diagram 
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APPENDIX  B 


DERIVATION 


The  filter  equations  will  be  derived  with  reference  to  Fig  .  A-i. 
The  equations  for  each  of  the  blocks  w u*  he  derived. 

B.  i  Block  1  -  State  Propagation 

The  dynamics  of  the  state  vector  propagation  are  given  by  the  non¬ 
linear  vector  function 


P>+U„ 

It  is  assumed  that  the  best  estimates  X  ,  and  p  are  available  such  that 

n-  1 

X  -~¥>(X  ,,£)  (1-2) 

n  n- 1  r 

B.  2  Block  2  -  Pseudo  State  Measurements 

A 

Let  Z  denote  the  actual  measurements  =  Z'  -  N  .  These  mea 
n  n  n 

surements  can  be  correlated  as  a  first-order  Markov  process  as 

N  =  pN  .  +  q  W  (2-1) 

n  r  n- 1  ^  n  ' 

The  development  in  this  report  can  be  extended  to  the  n^-order  Markov 
process.  Pseudo  state  measurements  are  now  constructed  as  nonlinear 
functions  of  the  actual  measurements  and  the  a  priori  state  estimate  Xfi. 

The  pseudo  state  measurements  then  become  linear  functions  of  the  state 
variables;  i.  e. 


£  =  i  (Z  , 
n  n  n 


X  .  T) 


n 


(2-2) 


B-l 


denotes  the  pseudo  state  measurements.  These  are  constructed  to  be  exactly 

linear  functions  of  the  state  variables  except  for  the  errors  A£n  that  result 

from  the  measurement  noise  N  ,  the  state  error  estimate  x  ,  and  the  mea- 

n  n 

surement  model  errors  T.  Hence,  in  the  equation 


t,  =  f  (X  )  -  At, 
*n  nv  n'  *n 


(2-3) 


i  denotes  a  linear  function  of  the  state  variables.  In  practice,  an  attempt 
is  made  for  i  to  be  identical  with  one  or  more  of  the  state  vector  compo¬ 
nents.  Pseudo  state  measurement  residuals  are  then  obtained  (Fig.  A-i) 

as  t,  -  f  (X  )• 
m  n'  n 

B-  3  Block  3  -  Set  Up 

The  computation  of  the  cost  function  requires  that  Rfi,  and 
C  be  calculated.  This  is  accomplished  by  using  the  inputs  to  Block  3  shown 
in  Fig.  A-l. 

The  computation  of  Rn,  using  Eq.  (2-1),  is  obtained  as 


R  =  <N-.  Nx> 
n  n  n' 


=  <(pNn-1  +  q  Wn)  pT  +  qT)> 

=  p<N  .  NT  .>  pT  +  q  <W  WT>  qT 
y  N  n- 1  n- 1  r  ^  n  n  n 


(3-1) 


r  pR  . p  +  q  q 


where  <W  WT>  =  I. 
n  n' 

The  computation  of  requires  the  evaluation  of  Xfi,  which  results  from 
Eqs.  (1-1)  and  (1-2)  as 


B-2 


(3-2) 


. "  «  -*sg saafe*ar». 


In  the  computation  of  Mn>  Eq.  (3-2)  is  used  as 


M  =  <X  XT> 
n  —  n  n 


=  <(A  e  .  +<2>p  +  U  )  (eT  .  iT  +  pT  0T  +  U?)> 
x'n  n-1  r  n  n- 1  n  r  n 

T  T  T  T 

=  4>  P  4  A  +  <j>  v  -  £2f  +  £^V  4  4» 

Tn  n-1  Yn  n-1  n-1 


(3-5) 


+  C$S'J'-  +  Q 


In  the  computation  of  Cn>  Eqs.  (3-2)  and  (3-4)  are  used  as 


C  =-<A£  XT'> 
n  x  n- 


=  -<G  <N  'xT>  -"/e  <X  XA>  -  <?  <f  Xx> 
nx  n  n'  nx  n  n'  n  n 


rsa  rp 
'v  \r 


hut  from  Eqs.  (3-2)  and  (2-1),  it  follows  that 


W„  5  <*n  Nn>  *  «♦„  V  l*^+  Un>  <NI  1  ^  +  K  *  T» 

s  4>  <e  ,  NT  ,  >  pT  -  4>  G  .  PT 
n  N  n-  1  n-1  Tn  n-1 


(3-6) 


Y  =  <Xn  TT>  =  A  L  . 
n  n  n  n- l 


(3-7) 


Hence 


C  =  M  -  (S  WT  -  S  YT 
n  n  n  n  n  n  n 


(3-8) 


where  it  is  assumed  that 


^<?NnT>  =<pfT)  =  <T  N*> 
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The  computation  of  2^  is  obtained  by  using  Eqs.  (3-4),  (3-2),  and  (2-1)  as 


2  =  <A£  AO 

=  <('Gn  N  +  X  +  &  T)  (Nj  <gI  +  +  TT  <?J)> 

"  n  n  nn  nnn  nn  n 


(3-9) 


-  'G  R  »*+  «  wjjfrf  +  J09  W  ST  +  .2?  M  «5^T 
nnn  nnn  nn  n  nnn 


+  .7?  Y  <§>T  +  <§>  YT^f’T  +  §  B  <g>T 
n  n  n  n  n  n  n  n 


B.  4  Block  4  -  The  Cost  Function 

The  cost  function  is  constructed  to  minimize  the  correlated  weighted 
'east  squares  of  the  sum  vector,  consisting  of  the  pseudo  state  measurement 
-esiduals  and  the  a  priori  state  residuals,  as 


i  -  t  (X  ) 
*n  n'  n' 


X  -  X 
n  n 


i.  e.  ,  the  two  vectors  are  joined  to  form  a  single  vector  consisting  of  the 
ordered  sum  of  all  components.  The  cost  function  then  becomes 


\  -  k  -  i  (X  )\T  Alt,  -  t  (X  ))  +  2(t,  -  f  (X  )\T  J  (X  -  X  ) 
n  \an  n  n  /  n\  n  n  n  /  \  n  n  n  /  n'  n  n 


+  (X  -  X  )AD  (X  -  X  ) 
'  n  n  n  n  n 


where  the  quantities  A,  J,  and  D  are  defined  as 


(4-1) 


A  J  \  /  2  C  V 1 

n  n  \  /  n  n  \ 


T 

J  D 
n  n 


T 

C  M 
n  n 


(4-2) 
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x  =  <(4  -  a  (X  ))  (4  -  i  (X  )}T>  =  <a 4  A4T> 

n  N\  n  nv  n7  \3n  n'  n'/  x  N  an  vl/ 

C  E  -  i  (X  ))  (X  -  X  )>  =  -<A4  XT> 
n  N\an  n'  n ')  v  n  n"  N  *n  r/ 

M  E  <(X  -  X  )  (X  -  X  )T>  =  <X  XT> 
n  N-v  n  n'  v  n  n'  '  n  n' 


are  each  calculated  in  Block  3.  Note  that  if  A 4  ^nd  X  -  X  are  Gaussian, 
the  cost  function  becomes  the  conditional  mean  and  the  estimate  is  considered 
optimal  in  the  usually  accepted  sense.  However,  these  residuals  need  not  be 
Gaussian  for  the  purposes  of  this  report.  The  cost  function  is  then  simply 
the  weighted  least  squares  cost  function.  Note  also  that  this  formulation  of 
the  cost  function  is  such  that,  given  Gaussian  variables,  the  distribution 
function  of  the  cost  function  is  chi-square  with  n^  +  n^  degrees  of  freedom  and 
that  these  data  could  be  transformed  to  orthogonal  variables.  If  this  were 
done,  each  item  in  the  sum  of  squares  would  be  weighted  inversely  as  its 
variance. 

Expressions  for  An>  D^,  and  Jr  will  be  calculated.  Multiplying 
both  sides  of  Eq.  (4-2)  together  and  equating  to  the  identity  matrix  results 
in  the  following  equations 

SA  +  CJT  =  I 


SJ  +  CD  =  C 


C  A  +  MD  =  I 


T  T 

C  A  +  MJ  -  O 


(4-3) 


whore  the  n  subscripts  have  been  omitted  for  ease  in  typing.  The  result  of 
solving  for  A,  D,  and  J  is 


A  =  [S  -  CM-1  CT]"1  =  2-i  +  2-1  CDCT  2"  1 


(4-4) 
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(4-5) 


D  =  [M  -CT  S"1  C]"1  =  M-1  +  M-1  CT  ACM-1 


and 


J  =  -  [2  -  C  M"1  CT]_1  CM'1 
=  -  Z'1  C[M  -  CT  2” 1  C]'1 


(4-6) 


There  are  two  solution  forms  for  each  expression  because  of  the  redundancy 
of  Eqs.  (4-3)  (4  equations  with  only  3  unknowns).  The  user  should  choose 
the  combination  that  provides  the  simplest  computer  program. 

B.  5  Block  5  -  The  Gain  Computation 

The  optimal  estimate  is  the  one  that  minimizes  the  cost  function 
obtained  in  Eq.  (4-1).  This  estimate  results  from  taking  the  gradient  of 
Eq.  (4-1) 


—•  =  -  2  f£T  A  U  -  f  (X  )\  +  2  JT  U  -  i  (X  )) 

9Xn  n  n\m  n'  n')  n  \*n  nv  n 7 


-  Z/£  J  (X  -  X  )  +  2  D  (X  -  X  ) 
n  n'  n  n-  n  n  n 


(5-D 


=  0  when  X  =  X 
n  n 


The  result  of  solving  Eq.  (5-1)  for  X  is 


(D  -^TJ)(X  -  X)=(^TA  -  JT)  U  ■  1  (X  )|  (5-2) 

'  n  n  n'  '  n  n  '  n  n  n'  \  n  n  n'j  v  ' 


Now,  since  i n(X  )  is  a  linear  function,  it  may  be  written  as 


i  (X  )  =  t  (X  )  +  (X  -  X  ) 
n'  n'  n'  n'  n'  n  n' 


(5-3) 


and  then  Eq.  (5-2)  may  be  written  as 


(5-4) 


where 


X 


n 


=  X  + 
n 


K  U  - 
nl  3n 


k  =  [d  +  xT  a  x 

n  L  n  n  n  n 


JT  £  -  J  ] 
n  n  n  nJ 


“ 1  [&T  A  -  JT]  (5-5) 
1  n  n  n J  '  ' 


Note  that  the  gain  matrix  Kn  consists  only  of  constants,  i.  e.  ,  is  inde¬ 
pendent  of  the  state  and  measurement  residuals. 

A 

The  gain  computed  in  Eq.  (5-5)  gives  an  estimate  Xr  that  minimizes 

the  weighted  least  squares  cost  function  of  Eq.  (4-1).  It  will  now  be  shown 

that  the  same  gain  Kn  also  minimizes  each  of  the  diagonal  elements  of  the 

error  covariance  matrix  P  .  This  assumes  that  the  minimum  variance  esti- 

n 

mate  is  a  linear  function  of  the  pseudo  state  measurement  residuals;  i.  e.  , 
assume  that 


=  X  +JC  U  - 
n  nl  n 


l  (X  )] 
n'  n'J 


(5-6) 


Then  the  estimation  error  is  obtained  by  adding  and  subtracting  X^  from 
both  sides  of  Eq.  (5-6)  to  obtain 


e  =  X  -  CfC  [  t,  (Z  ,  X  ,  T)  -  t,  (Z  ,  X  ,  T) 
n  n  nl  3nv  n  n  '  n  n 


+  £  (X  )  -  <£  (X  )] 
n'  n'  n'  n'J 


(5-7) 


Note  that  Eq.  (5-7)  agrees  with  Eq.  (5-4)  since  the  identity 


i£  (X  )  -  £,  (Z 
nv  n' 

has  been  added  within  the  brackets. 
Eq.  (3-4)  as 


* 

,  X  , 
n  n 


Hence, 


T)=  0 

Eq.  (5-7)  may  be  written  using 


e 

n 


=  X  +  etc  [At,  -  <£  X  ] 
n  nl  sn  n  nJ 


(5-8) 
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I 


t  * 


g  ? 


.Tv 


The  error  covariance  matrix  of  the  estimate  is  P  =  <e  e  >.  It  is  easily 

n  N  n  n'  J 


shown  by  direct  differentiation  that  each  of  the  diagonal  terms  in  PR  is 


minimized  by  choosing to  satisfy  the  Weiner  Hopf  equation  as 


<X  (Af,  -  <£  X  )T>  =  -,X'<(A£  -  SB  X  )  (At,  -  SB  X  )T>  (5-9) 

nl  5n  n  n'  '  -  nNV  an  n  n'  '  an  n  t£  '  .  ' 


An  interesting  derivation  of  the  Wiener  Hopf  equation  is  given  by  Johnson 
(Ref.  3).  Equation  (5-9)  may  be  expanded  and  the  definitions  of  the  covari¬ 
ance  functions  used  to  obtain 


ct  +  m  sbt  =jc  [e  +  se  cT  +  c  sb1  +  sb  m  seT] 

n  nn  nln  nn  nn  nnnJ 


(5- 10) 


It  will  now  be  shown  that  the  defined  in  Eq.  (5- 10)  is  exactly  the  same  as 


the  specified  in  Eq.  (5-5);  i.  e. , 


K  ziCfC 
n  n 


This  is  done  by  substituting  from  Eq.  (5-5)  forJif^  in  Eq.  (5-10).  The 


result  of  this  substitution  is 


[D  +  SB1  A  SB-  JT  SB  -  <BT  J]  [M  +  CT] 


=  [SBT  A  -  JT]  [  2  +  C  SB1  +  SB  CT  +  SB  M  SBT] 


(5-11) 


It  is  easily  shown,  using  Eqs.  (4-3),  (4r4),  (4-5),  and  (4-6),  that  Eq.  (5-11) 


is  an  identity.  The  proof  is  simplified  by  noting  that  corresponding  terms 


involving  similar  groupings  of  SB  and  SB  must  vanish  identically.  Hence  it 
has  been  shown  that  the  weighted  least  squares  estimate  is  identical  with  the 
linear  minimum  variance  estimate  and  that  the  simpler  expression  may  be 
used  for  the  gain  computation  as 


K  =  [CT  +  M  SBTUZ  +  SB  CT  +  C  f£T  +  S£M  SBT]~l  (5-12) 
n.n  nnJLn  nn  nn  n 


B-9 


.  w  Y  'iV  ’  nth  h  ill 


In  the  computation  of  P  ,  Eq.  (5-8)  is  used  as 


e  =  X  +  K  At 
n  n  n  n 


(5-13) 


where 


Ar  =  At,  -  <£  X 
n  ~  3n  n  n 


(5-14) 


Then 


E  <(X  +  K  A r  )  (X  +  K  Ar)S 
--n-  n  n  n7  '  n  n  n7  ' 


-  <X  X1)  +  K  <A.r  Xr>  +  ^X  A rT>  KT  +  K  <ArAri>Ki 
N  n  n'  nx  n  w'  x  n  n'  n  nx  n 


TWT 


(5-15) 


where 


<Ar  X  >  a  <(A£  -  .£  X  )  X1) 
x  n  n'  N'  n  n7  n 


=  <At,  X1)  -  <£  <X  XS 
3n  n'  n  n  n ' 


(5-16) 


=  -C  -  <£  M 
n  n  n 


<Ar  A  rT>  ~<(At,  -  !£  X  )  (A  t,  -  <£  X  )T> 

N  n  x\'  N  sr.  n  n7  '  m  n  n7 

-  £  +  £  CT  +  C  .f£T  +  S£  M  ,£T 
n  n  n  n  n  n  n 


(5-17) 


Equation  (5-15)  may  be  simplified  by  noting  that  the  Wiener  Hopf  equation 
(5-9)  requires 


<X  A  rT>  =  -K  <Ar  A  rT> 
N  n  n'  nN  n  n 


(5-18) 
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_'-uAc.."&&.i-i  st^Dfiaasoar.i's  -.  .^j.-  -^tssr.z 


Hence,  Pn  simplifies  to 


P  =  <X  xb  +  K  <Ar  X*> 
n  nn  n N  n  n 


=  M  -  K  [C  +.S?  M] 
n  n  1  n  n  nJ 


(5-19) 


=  (I  -  K  £  ]  M  -  K  C 
L  n  nJ  n  n 


Pn  may  also  be  computed  using  Eq.  (5-15),  which  does  not  require  that  the 
filter  satisfy  the  Wiener  Hopf  equation.  The  result  is 


Pn  =  M  -  K  ( C  +  £  M.  ]  -  [Cj  +  M  aF\  kJ 
n  n  nn  nn  Ln  n  n 

+  K  [Z  +  £  CT  +  C  &  \  £  M  £T]  KT 
n  1  n  nn  nn  nnn  n 


(5-20) 


B.  6  Block  6  -  Step  Initialization 

The  quantities  Gn>  Vn>  and  are  computed  in  Block  6.  These 
quantities  are  required  for  the  set  up  computations  in  Block  3. 

The  computation  of  these  three  covariance  matrices  requires  com¬ 
putation  of  the  estimation  error.  This  is  obtained  in  Eq.  (5-12)  as 


e  =X  +  K  [At,  -  £  X  ] 
n  n  n  n  n  n 


The  use  of  Eq.  (3-3)  permits  Eq.  (6-1)  to  be  written  as 


e  =  X  +  K  [<3  N  +  .X?  X  +<?  T  -  £  X  ] 
n  n  nLnn  nnn  nnJ 


[I+K  ^  -  K  &  ]  X  +  K  <3  N  +  K  S  T 
n  n  n  nJ  n  nnn  n  n 


FX+K0N+K£T 
n  n  nnn  n  n 
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(6-1) 


(6-2) 


(6-3)' 


where 


F  E  1  +  K  ,2$?  -  K  <5?  (6-4) 

n  n  n  n  n 

Then  is  obtained  by  using  its  definition  and  Eq.  (3-5)  as 

G  E  <e  N^> 

_  n  '  n  n 

*  «F„  +  K„  ®n  N„  +  Kn  *n  ¥>  '  Nn>  <6‘5> 

*  F<S"  Nn>  +  Kn  ®„<N„ 

=  FW  +  K  (8  R  (6-6) 

n  n  n  n 

The  matrix  Vn  is  obtained  using  Eqs.  (6-3)  and  (3-2)  as 

V  =  <e  pT> 

n  ~  N  n  v  ' 

=  <(F  X  +  K  8  N  +  K  &  T  )  pT> 
x  n  n  n  n  n  n  n  x'  r  ' 

-  F  <(4>  e  .+®p  +  U)pT>  (6-7) 

nNVTn  n-1.  v  n'  r  . 

=  Fn+n<Vl?T>+Fn®<PeT> 

*Fn+„Vn-l  +  Fn®S 
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The  matrix  is  computed  using  Eqs.  (6-3)  and  (3-7)  as 


L  E  <e  T  > 
n  N  n  ' 


=  <f(F  Xn  +  K  ®  N  +  K  <T  T)  Tx> 
'  n  n  nnn  nn 


.<v  ^'T  .'V  rvT. 

=  F  <X  T  >  +  K  &  <T  T  > 
nx  n  '  n  n  N 


(6-8) 


=  F  Y+  K  &  B 
n  n  n  n 


B.  7  Block  7  -  Filter  Performance 


The  filter  performance  may  be  verified  by  computing  the  cost 

function  given  as  Eq.  (4-1)  evaluated  at  X  =  X  and  also  at  X  .  The 
°  n  n  n 

equations  are 


S,  =  K  -  Wf  A„(^n  '  '„<Xn>) 


(7-1) 


X  =  k  -  £  (X  )'jT  A  k  -l(X)|tZ (c,  -  t  (X  ))T  J  (X  -  X  ) 
n  \  n  nv  n ')  n  ^n  n'  n7/  Vn  n'  n 7  n'  n  n7 


(7-2) 


+  (X  -  X  )T  D  (X  -  X  ) 
'  n  n7  n  n  n7 


It  is  necessary  to  verify  that 


X  <  X 
n  n 


(7-3) 


in  order  to  justify  processing  the  measurements.  In  the  event  that  this  con¬ 
dition  is  not  satisfied,  the  basic  real-world  dynamics  and  measurement 
models  should  be  checked.  The  computer  program  should  come  to  a  halt 
whenever  Eq.  (7-3)  is  not  satisifed. 
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